package com.rockwell.robot2015.model.states.running.sequence;

import com.rockwell.robot2015.model.MazeType;
import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.detectors.TreasureDetector;
import com.rockwell.robot2015.model.sensors.IRobotSensors;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;

public class ApproachingMazeSequence extends RobotRunningSequence {
	/**
	 * Alignment threshold beyond which we should adjust the orientation of the
	 * robot.
	 */
	private final static double ALIGNMENT_THRESHOLD = 5;

	private final static int BLOCK_ANGLE_THRESHOLD = 10;

	private final static int AWAY_BLOCK_ANGLE = 20;
	
	/**
	 * big moving distance
	 */
	public static int BIG_MOVE_INDEX = 30; // cm

	/**
	 * small moving distance
	 */
	public static int SMALL_MOVE_INDEX = 20; // cm

	static MazeType m_mazeType;
	
	enum State implements IState {
		Start {

			@Override
			public void enter(IStateMachine machine) {
			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().log("Start approaching maze", 2);
				m_mazeType = checkMaze(machine);
				
				machine.robot().getSensor().setInMaze(false);

				machine.robot().setCurrentMaze(m_mazeType);
				machine.changeState(AdjustOrientation);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		AdjustOrientation {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Adjust orientation", 4);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();

				IDetector mazeDetector = RobotRunningSequence.getMazeDetector(robot);

				if (mazeDetector != null) {
					/*
					 * Point[] line = mazeDetector.getLine();
					 * 
					 * if (line[0] != null && line[1] != null) { double dx =
					 * line[0].x - line[1].x; double dy = line[0].y -
					 * line[1].y;
					 * 
					 * int degree = (int)Math.toDegrees(Math.atan2(dx, dy));
					 * degree /= 2;
					 * 
					 * String log = String.format("Turning degree: %d",
					 * degree); robot.log(log, 4);
					 * 
					 * if (Math.abs(degree) > ALIGNMENT_THRESHOLD) {
					 * robot.sendCommand(RobotAction.TurnDegree, degree);
					 * robot.sleep(Robot.MOTION_SETTLING_TIME); } else {
					 * machine.changeState(Move); } } else {
					 * machine.changeState(Move); }
					 */
					if (mazeDetector.isPresent()) {
						/*
						Rect box = mazeDetector.getBoundingRect();
						if (box != null) {
							int vpwidth = mazeDetector.getWidth();
							int vpheight = mazeDetector.getHeight();
							
							int bottom = box.y + box.height;
							int spaceTop = box.y;
							int spaceBottom = vpheight - bottom;
							
							String log = String.format("Screen: (width:%d, height:%d), Box: (x:%d, y:%d, width:%d, height:%d)",
									vpwidth, vpheight, box.x, box.y, box.width, box.height);
							
							robot.log(log, 4);
							
							/if (box.height < vpheight / 3) {
								// The target is too small in the screen 
								machine.changeState(MazeOutofSight);
							}
							else/ {
								if (spaceTop > vpheight / 3 && spaceBottom < vpheight / 10) {
									// turn left
									robot.turn(10);
									robot.sleep(Robot.MOTION_SETTLING_TIME);
									machine.changeState(Move);
								}
								else if (spaceBottom > vpheight / 3 && spaceTop < vpheight / 10) {
									// turn right
									robot.turn(-10);
									robot.sleep(Robot.MOTION_SETTLING_TIME);
									machine.changeState(Move);
								}
								else {
									int d = RobotRunningSequence.getOrientation(robot, mazeDetector);

									log = String.format("Turning degree: %d", d);
									robot.log(log, 4);

									if (Math.abs(d) > ALIGNMENT_THRESHOLD) {
										robot.sendCommand(RobotAction.TurnDegree, d);
										robot.sleep(Robot.MOTION_SETTLING_TIME);
									} else {
										machine.changeState(Move);
									}
								}
							}
						}
						else*/ {
							int d = RobotRunningSequence.getOrientation(robot, mazeDetector);

							String log = String.format("Turning degree: %d", d);
							robot.log(log, 4);

							if (Math.abs(d) > ALIGNMENT_THRESHOLD) {
								robot.sendCommand(RobotAction.TurnDegree, d);
								robot.sleep(Robot.MOTION_SETTLING_TIME);
							} else {
								machine.changeState(Move);
							}
						}
					} else {
						machine.changeState(MazeOutofSight);
					}
				} else {
					machine.changeState(MazeOutofSight);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}
		},
		Move {
			@Override
			public void enter(IStateMachine machine) {
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();

				TreasureDetector treasureDetector = (TreasureDetector)robot.getTreasureDetector();
				
				IDetector mazeDetector = RobotRunningSequence
						.getMazeDetector(robot);

				m_mazeType = checkMaze(machine);
				
				if (treasureDetector.isPresent()) {
					int distance = (int)treasureDetector.getDistanceInPercentage();
					int degree = RobotRunningSequence.getOrientation(robot, treasureDetector);

					String log = String.format("Treasure found, distance: %d, angle: %d",
							distance, degree);
					
					robot.log(log, 4);
					
					if (Math.abs(degree) < BLOCK_ANGLE_THRESHOLD) {
						machine.changeState(MazeBlocked);
						return;
					}
				}

				IRobotSensors sensor = robot.getSensor();

				if (mazeDetector.isPresent()) {
					robot.sendCommand(RobotAction.MoveWithSensor, BIG_MOVE_INDEX);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
					
					if (sensor.detectFrontWall()) {
						machine.changeState(MazeWallReached);
					}
					else {
						if (sensor.detectLeftWall() && sensor.detectRightWall()) {
							robot.sendCommand(RobotAction.MoveWithSensor, 5);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
							
							if (sensor.detectLeftWall() && sensor.detectRightWall()) {
								
								/*
								robot.turn(10);
								robot.sleep(Robot.MOTION_SETTLING_TIME);
								
								IDetector detector = RobotRunningSequence.getMazeDetector(robot);
								
								if (detector == mazeDetector) {
									robot.turn(-20);
									robot.sleep(Robot.MOTION_SETTLING_TIME);
									
									detector = RobotRunningSequence.getMazeDetector(robot);
									
									robot.turn(10);
									robot.sleep(Robot.MOTION_SETTLING_TIME);
									
									if (detector == mazeDetector) {
										// The robot is already in a maze
										machine.changeState(AlreadyInMaze);
										return;
									}
								}
								*/
								
								machine.changeState(AlreadyInMaze);
							}
						}
						
						machine.changeState(AdjustOrientation);
					}
				}
				else {
					if (sensor.detectFrontWall()) {
						machine.changeState(MazeWallReached);
					}
					else {
						machine.changeState(MazeOutofSight);
					}
				}
				/*
				if (robot.isJogDone()) {
					if (sensor.detectFrontWall()) {
						machine.changeState(MazeWallReached);
						return;
					}
					else {
						machine.changeState(AdjustOrientation);
						return;
					}
				}
				else {
					if (mazeDetector.isPresent()) {
						Rect rect = mazeDetector.getBoundingRect();
						Point center = mazeDetector.getCenter();
						int imageHeight = mazeDetector.getHeight();
						
						if (rect != null) {
							if (rect.y + rect.height < imageHeight / 3) {
								robot.log("Adjust -5 degrees", 2);
								
								robot.sendCommand(RobotAction.Stop);
								
								robot.sendCommand(RobotAction.TurnDegree, -5);
								robot.sleep(Robot.MOTION_SETTLING_TIME);
								
								robot.startJogging();
							}
							else if (rect.y > imageHeight * 2 / 3) {
								robot.log("Adjust 5 degrees", 2);
								
								robot.sendCommand(RobotAction.Stop);
								
								robot.sendCommand(RobotAction.TurnDegree, 5);
								robot.sleep(Robot.MOTION_SETTLING_TIME);
								
								robot.startJogging();
							}
						}
					}
					else {
						machine.changeState(MazeOutofSight);
					}
				}
				*/
			}

			@Override
			public void exit(IStateMachine machine) {
				//machine.robot().log("Exit ApproachingMaze.Jog", 2);
			}

		},
		MazeBlocked {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log(
						"Maze blocked by a treasure or an obstacle", 2);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();

				IDetector blockDetector = null;

				if (robot.getTreasureDetector().isPresent()) {
					blockDetector = robot.getTreasureDetector();
				} else {
					machine.changeState(AdjustOrientation);
					return;
				}

				if (blockDetector != null) {
					int d = RobotRunningSequence.getOrientation(robot, blockDetector);

					if (Math.abs(d) < BLOCK_ANGLE_THRESHOLD) {
						if (d > 0) {
							d = d - AWAY_BLOCK_ANGLE;
						}
						else {
							d = AWAY_BLOCK_ANGLE + d;
						}
						
						robot.turn(d);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
						
						robot.sendCommand(RobotAction.MoveWithSensor, 20);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
					}
					else {
						machine.changeState(AdjustOrientation);
					}
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		MazeWallReached {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Maze Reached", 2);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				IRobotSensors sensor = robot.getSensor();
				
				if (sensor.detectLeftWall()) {
					boolean canAdjust = robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
					
					if (!canAdjust) {
						IRobotSensors.LeftAngle angle = sensor.getLeftAngle();
						if (angle.leftFrontOpen) {
							robot.sendCommand(RobotAction.MoveWithoutSensor, -5);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
						else if (angle.leftBackOpen) {
							robot.sendCommand(RobotAction.MoveWithoutSensor, 5);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
							
						turnRight2by45(robot, sensor);
					}
				}
				else if (sensor.detectRightWall()) {
					turnLeft135_45(robot, sensor);
				}
				else {
					if (sensor.isFrontTooClose()) {
						robot.sendCommand(RobotAction.MoveToTreasure, -1);
						machine.robot().sleep(Robot.MOTION_SETTLING_TIME);
					}
					else {
						//robot.sendCommand(RobotAction.MoveWithSensor, 2);
						//machine.robot().sleep(Robot.MOTION_SETTLING_TIME);
					}
					
					turnRight2by45(robot, sensor);
				}
				
				if (sensor.detectLeftWall()) {
					machine.changeState(AdjustAlignment);
				}
				else {
					machine.changeState(MazeOutofSight);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
			}

		},
		/**
		 * Adjust the alignment along the wall.
		 */
		AdjustAlignment {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Adjust alignment", 4);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();

				robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);

				IRobotSensors sensor = robot.getSensor();
				
				if (sensor.detectLeftWall()) {
					machine.changeState(Aligned);
				}
				else {
					machine.changeState(MazeOutofSight);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		MazeOutofSight {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Maze out of sight", 2);
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		AlreadyInMaze {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Already in maze", 2);
				
				machine.robot().getSensor().setInMaze(true);
				machine.robot().setCurrentMaze(m_mazeType);
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		EntranceFoundFromFarAway {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Aligned {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Approaching Maze done", 2);
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		};
		static ApproachingMazeSequence m_sequence;

		public static void setSequence(ApproachingMazeSequence sequence) {
			m_sequence = sequence;
		}
	}

	private static void turnRight2by45(Robot robot,	IRobotSensors sensor) {
		turnTwice(robot, sensor, -45, -45);
	}

	private static void turnLeft135_45(Robot robot,	IRobotSensors sensor) {
		turnTwice(robot, sensor, 135, 45);
	}

	private static void turnTwice(Robot robot, IRobotSensors sensor, int angle1, int angle2) {
		robot.turn(angle1);
		robot.sleep(Robot.MOTION_SETTLING_TIME);
		
		if (sensor.detectLeftWall()) {
			robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
			robot.sleep(Robot.MOTION_SETTLING_TIME);
		}
		else {
			robot.turn(angle2);
			robot.sleep(Robot.MOTION_SETTLING_TIME);
		}
	}
	
	public ApproachingMazeSequence(RobotRunningStateMachine running) {
		super(running, State.Start);

		State.setSequence(this);

		m_currState = State.Start;
	}

	@Override
	public void start() {
		//m_running.robot().log("Approaching maze", 1);
		m_currState = State.Start;
		m_nextState = null;
		
		Robot robot = m_running.robot();
		robot.enableTreasureDetector(true);
		//robot.enableObstacleDetector(true);
		robot.enableMaze1Detector(true);
		robot.enableMaze2Detector(true);
		robot.enableMaze3Detector(true);
		robot.enableMazeEntranceGuideDetector(true);
		robot.enableDropAreaDetector(false);
		robot.enableOuterWallDetector(true);
	}

	@Override
	public boolean succeeded() {
		return m_currState.equals(State.Aligned) || m_currState.equals(State.AlreadyInMaze);
	}

	public boolean isAligned() {
		return m_currState.equals(State.Aligned);
	}
	
	public boolean alreadyInMaze() {
		return m_currState.equals(State.AlreadyInMaze);
	}
	
	@Override
	public boolean failed() {
		return m_currState.equals(State.MazeOutofSight);
	}
	
	public MazeType getMazeType() {
		return m_mazeType;
	}
} 
